//
// Created by dwx on 2024/4/16.
//
#include "mavlink_crc.h"

#define X25_INIT_CRC 0xffff

void crc_init(uint16_t* crcAccum) {
    *crcAccum = X25_INIT_CRC;
}

uint16_t crc_accumulate(uint8_t data, uint16_t* crcAccum) {
    uint8_t tmp;

    tmp = data ^ (uint8_t)(*crcAccum &0xff);
    tmp ^= (tmp<<4);
    *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4);
    return *crcAccum;
}
static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
uint16_t crc_calculate(uint8_t* buffer, int length, uint8_t msgid) {
    int i;
    uint16_t crc;
    crc_init(&crc);
    // 缓冲区内的数据
    for (i = 0; i < length; i++) {
        crc = crc_accumulate(buffer[i], &crc);
    }

    // CRC_EXTRA
   return  crc_accumulate(mavlink_message_crcs[msgid], &crc);


}

uint16_t  generate_vector(uint8_t* buffer, int length, uint8_t msgid){
    uint8_t  data[1024] ;
    memcpy(data,&buffer[1],length-1);
    return crc_calculate(data,length-1,msgid);
}